#include "Menu.h"
#include "lcd.h"
#include "includes.h"
#include "uart.h"
#include "PID.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <avr/interrupt.h>

unsigned char PID_op_flag = 0,buff[50];
STR_EXTREMUM extremum;
float this_target = 0.0;
float next_target = 0.0;

PID_PARAMS params =
{
   0.00,              /* 'P' proportional gain          */
   0.00,              /* 'I' integral gain              */
   0.00,              /* 'D' derivative gain            */
   0.00,              /* 'A' acceleration feed forward  */
   0.00,              /* 'F' friction feed forward      */
   0.00,              /* 'V' velocity feed forward      */
   0.00,              /* 'H' output Hold                */
   0.00,              /* 'B' bias                       */
   0.00,              /* 'R' acceleration rate          */
   0.00,              /* 'S' set point                  */
   0.00,              /* 'T' transfer ratio             */
   0.00,              /* 'L' lag in output change       */
   0.00,              /* 'N' minimum output value       */
   0.00,              /* 'M' maximum output value       */
   0.00,              /* 'Y' repeat cycle count         */
   0.00,              /* 'W' slew limit                 */
};

void start_automation_setting (void)
{

  //cli();
			 TIMSK &= ~(1<<TOIE0);
			 TIMSK &= ~(1<<TOIE2);
			 PID_op_flag = 0;  
 extremum.temperature_max = 0;
 extremum.temperature_min = 10000;
 extremum.op_max = 0;
 extremum.op_min = 10000;	

 LCDSendCommand(CLR_DISP);
 LCDSendCommand(DD_RAM_ADDR+2);
 LDCsendStr ("Cooling");

  cooling_heating_cycles(cooling,&extremum);
 
 LCDSendCommand(CLR_DISP);
 LCDSendCommand(DD_RAM_ADDR+2);
 LDCsendStr ("Heating");

  cooling_heating_cycles(heating,&extremum);

  str_PID.set_temperature = (extremum.temperature_max + extremum.temperature_min)/2;
  str_PID.set_op = (extremum.op_max/2);
  
  if (extremum.temperature_min > extremum.temperature_max) str_PID.edge = 0;
  else str_PID.edge = 1;

 LCDSendCommand(CLR_DISP);
 LCDSendCommand(DD_RAM_ADDR);
 LDCsendStr ("Set temp: ");

  LCDSendChar((str_PID.set_temperature/ 1000) + '0');
  LCDSendChar(((str_PID.set_temperature%1000) / 100) + '0');
  LCDSendChar('.');
  LCDSendChar(((str_PID.set_temperature%100) / 10) + '0'); 
  LCDSendChar((str_PID.set_temperature % 10) + '0');

 LCDSendCommand(DD_RAM_ADDR2+8);
 
 PID_init(0.2,0.05,0,-160,+160,str_PID.set_temperature);
 {
  Delay(1200000);
  read_adcs();
   LCDSendChar((ADC_data.Temp/ 1000) + '0');
   LCDSendChar(((ADC_data.Temp%1000) / 100) + '0');
   LCDSendChar('.');
   LCDSendChar(((ADC_data.Temp%100) / 10) + '0'); 
   LCDSendChar((ADC_data.Temp % 10) + '0');
  PID_update(ADC_data.Temp);
  LCDSendCommand(DD_RAM_ADDR2+8);
 }while(ADC_data.Temp<str_PID.set_temperature-20 || ADC_data.Temp >str_PID.set_temperature + 20);

  //sprintf(buff,"temerature setting is over");
  //PutStringUART0 (buff);

  PID_op_flag = 1;  
  PID_init(0.5108,0.0245,0,-160,+160,str_PID.set_op);

  //sprintf(buff,"set temperature = %d\r",str_PID.set_temperature);
  //PutStringUART0 (buff);
  //sprintf(buff,"set operating voltage= %d\r",str_PID.set_op);
  //PutStringUART0 (buff);
  //sprintf(buff,"edge= %d\r",str_PID.edge);
  //PutStringUART0 (buff);
  
  //sei();  
  			 TIMSK |= 1<<TOIE0;
			 TIMSK |= 1<<TOIE2;
  show_menu_on_LCD();	
}

void stop_automation_setting(void)
{
 PID_op_flag = 0;
}

void PID_init(float kp, float ki, float kd,short min,short max,short setpt )
{
 /*
 m_kp = kp;
 m_ki = ki;
 m_kd = kd;
 m_error_trash = error_thrash;

 m_h = step_time;
 m_inv_h = 1. / step_time;

 m_integral = 0;
 m_started = 0;
 */
  params.p_gain = kp;     /* 'P' proportional gain          */
  params.i_gain = ki;     /* 'I' integral gain              */
  params.d_gain = kd;     /* 'D' derivative gain            */
  //params.acc_ff         /* 'A' acceleration feed forward  */
  //params.fri_ff         /* 'F' friction feed forward      */
  //params.vel_ff         /* 'V' velocity feed forward      */
  //params.hold           /* 'H' output Hold                */
  //params.bias           /* 'B' bias                       */
  //params.accel          /* 'R' acceleration rate          */
  params.setpt = setpt;   /* 'S' set point                  */
  //params.trans          /* 'T' transfer ratio             */
  //params.lag            /* 'L' lag in output change       */
  params.min = min;       /* 'N' minimum output value       */
  params.max = max;       /* 'M' maximum output value       */
  //params.cycles         /* 'Y' repeat cycle count         */
  //params.slew           /* 'W' maximum slew rate          */

}
/*
void PID_update (short error)
{
 
 float q,deriv,actuate;
 //unsigned char buff[50];


 if (abs(error) < m_error_trash) q = 1.;
 else q = 0;

 m_integral += m_h*q*error;
 
 if (!m_started)
 {
  m_started = 1;
  deriv = 0;
 }
 else 
 deriv = (error - m_prev_error)*m_inv_h;
 
 m_prev_error = error;
 actuate = (float)(m_kp*(error + m_ki*m_integral + m_kd*deriv));
 
 if (actuate > 160.) actuate = 160.;
 else if (actuate < -160.) actuate = -160.;


 sprintf (buff,"%04d",ADC_data.OP_point);
 PutStringUART0 (buff);
 sprintf (buff,"%04d",(short)(actuate));
 PutStringUART0 (buff);

 if (actuate >= 0) DAC_data.PLT_current=(unsigned short) 2095-(unsigned short)(12.323*abs(actuate)); //-
 if (actuate < 0)  DAC_data.PLT_current=(unsigned short) 2095+(unsigned short)(11.764*abs(actuate)); //+
  set_dac_data (DAC_data.PLT_current,SET_PLT_CURRENT);
}
*/
void cooling_heating_cycles (unsigned char mode,STR_EXTREMUM* structure)
{
short temp1,temp2;
unsigned char /*buff[50],*/i;

do
 {
  if (mode) set_dac_data (4095,SET_PLT_CURRENT);
  else set_dac_data (500,SET_PLT_CURRENT);

  read_adcs();
  temp1 = ADC_data.Temp;
   if (ADC_data.OP_point > structure->op_max) structure->op_max = ADC_data.OP_point,structure->temperature_max = temp1;
   if (ADC_data.OP_point < structure->op_min) structure->op_min = ADC_data.OP_point,structure->temperature_min = temp1;
  
  sprintf(buff,"%04d",temp1);
  PutStringUART0 (buff);
  sprintf (buff,"%04d",ADC_data.OP_point);
  PutStringUART0 (buff);

  LCDSendCommand(DD_RAM_ADDR2+8);
  	for (i=0;i<2;i++)
	 {
	  LDCsendStr(">  ");
	  Delay(800000);
	  LCDSendCommand(DD_RAM_ADDR2+8);
	  LDCsendStr(" > ");
	  Delay(800000);
	  LCDSendCommand(DD_RAM_ADDR2+8);
	  LDCsendStr("  <");
	  Delay(800000);
	  LCDSendCommand(DD_RAM_ADDR2+8);
	  LDCsendStr(" < ");
	  Delay(800000);
	  LCDSendCommand(DD_RAM_ADDR2+8);
	 }
  read_adcs();
  temp2 = ADC_data.Temp;
   if (ADC_data.OP_point > structure->op_max) structure->op_max = ADC_data.OP_point,structure->temperature_max = temp2;
   if (ADC_data.OP_point < structure->op_min) structure->op_min = ADC_data.OP_point,structure->temperature_min = temp2;
  //sprintf(buff,"temp2 = %d\r",temp2);
  //PutStringUART0 (buff);
 }while( abs(temp1 - temp2)>1 && temp1 < 4800);
 /*
  sprintf(buff,"temperature_max = %d\r",structure->temperature_max);
  PutStringUART0 (buff);
  sprintf(buff,"temperature_min = %d\r",structure->temperature_min);
  PutStringUART0 (buff);
  sprintf(buff,"op_max = %d\r",structure->op_max);
  PutStringUART0 (buff);
  sprintf(buff,"op_min = %d\r",structure->op_min);
  PutStringUART0 (buff);
  */
}

void PID_update (short PV)
{

  /* the three static variables are required to retain  */
  /* information between passes through the loop        */
  static float     integral                = 0.0;
  static float     last_error              = 0.0;
  static float     last_output             = 0.0;

         float     this_error;
         float     this_output;
         float     accel;
         float     deriv;
         float     friction;

  /* the desired PV for this iteration is the value     */
  /* calculated as next_target during the last loop     */
  this_target = next_target;
  
  /* test for acceleration, compute new target PV for   */
  /* the next pass through the loop                     */
  if (params.accel > 0 && this_target != params.setpt)
  {
    if (this_target < params.setpt)
    {    
      next_target += params.accel;
      if (next_target > params.setpt)
      {
        next_target = params.setpt;
      }
    }
    else /* params.target > params.setpoint */
    {    
      next_target -= params.accel;
      if (next_target < params.setpt)
      {
        next_target = params.setpt;
      }
    }
  }
  else
  {
    next_target = params.setpt;
  }
  
  /* acceleration is the difference between the PV      */
  /* target on this pass and the next pass through the  */
  /* loop                                               */
  accel = next_target - this_target;

  /* the error for the current pass is the difference   */
  /* between the current target and the current PV      */
  this_error = this_target - PV;

  /* the derivative is the difference between the error */
  /* for the current pass and the previous pass         */
  deriv = this_error - last_error;

  /* a very simple determination of whether there is    */
  /* special friction to be overcome on the next pass,  */
  /* if the current PV is 0 and the target for the next */
  /* pass is not 0, stiction could be a problem         */
  friction = (PV == 0.0 && next_target != 0.0);

  /* the new error is added to the integral             */
  integral += this_target - PV;

  /* now that all of the variable terms have been       */
  /* computed they can be multiplied by the appropriate */
  /* coefficients and the resulting products summed     */
  this_output = params.p_gain * this_error
              + params.i_gain * integral
              + params.d_gain * deriv
              + params.acc_ff * accel
              + params.vel_ff * next_target
              + params.fri_ff * friction
              + params.bias;

  last_error   = this_error;

  /* check for slew rate limiting on the output change  */
  if (0 != params.slew)
  {
    if (this_output - last_output > params.slew)
    {
      this_output = last_output + params.slew;
    }
    else if (last_output - this_output > params.slew)
    {
      this_output = last_output - params.slew;
    }
  }

  /* now check the output value for absolute limits     */
  if (this_output < params.min)
  {
    this_output = params.min;
  }
  else if (this_output > params.max)
  {
    this_output = params.max;
  }
 if (!str_PID.edge) this_output = -this_output;

 //sprintf (buff,"%04d",ADC_data.OP_point);
 //PutStringUART0 (buff);
 //sprintf (buff,"%04d",this_output);
 //PutStringUART0 (buff);

 if (this_output >= 0) DAC_data.PLT_current=(unsigned short) 2095-(unsigned short)(12.323*abs(this_output)); //-
 if (this_output < 0)  DAC_data.PLT_current=(unsigned short) 2095+(unsigned short)(11.764*abs(this_output)); //+
  set_dac_data (DAC_data.PLT_current,SET_PLT_CURRENT);
}
